# 平面分割
# 随机抽样一致性 RANSAC算法   https://zhuanlan.zhihu.com/p/402727549

import open3d as o3d

pcd_point_cloud = o3d.data.PCDPointCloud()
pcd = o3d.io.read_point_cloud(pcd_point_cloud.path)

# 执行 RANSAC 平面分割
#      参数 distance_threshold 距离阈值 若一个点到拟合平面的距离≤0.01，则被视为 “内点”（属于该平面）；否则为 “外点”
#      参数 ransac_n 每次随机采样多少个点来拟合一个平面
#      参数 num_iterations 迭代次数
#  返回 plane_model：平面方程的参数(a, b, c, d)，满足平面方程ax + by + cz + d = 0
#  返回 inliers：内点的索引列表，记录哪些点属于该平面
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=3,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

# 提取内点云 
# 通过select_by_index(inliers)函数，从原始点云pcd中筛选出 内点
inlier_cloud = pcd.select_by_index(inliers)
# 给内点云着色
inlier_cloud.paint_uniform_color([1.0, 0, 0])
# 提取外点云
outlier_cloud = pcd.select_by_index(inliers, invert=True)

o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud],
                                  zoom=0.8,
                                  front=[-0.4999, -0.1659, -0.8499],
                                  lookat=[2.1813, 2.0619, 2.0999],
                                  up=[0.1204, -0.9852, 0.1215])
